/*
 * Joystick.cpp
 *
 *  Created on: Jan 29, 2010
 *      Author: fmason
 */

#include "Joystick.h"

void Joystick::setup()
{
	//set baud rate for serial output
	Serial.begin( BAUD_RATE );

	pinMode( LEFT_JOYSTICK_POWER_PIN, OUTPUT );
	pinMode( RIGHT_JOYSTICK_POWER_PIN, OUTPUT );
	pinMode( SERVO_JOYSTICK_POWER_PIN, OUTPUT );
	digitalWrite( LEFT_JOYSTICK_POWER_PIN, HIGH );
	digitalWrite( RIGHT_JOYSTICK_POWER_PIN, HIGH );
	digitalWrite( SERVO_JOYSTICK_POWER_PIN, HIGH );


}

void Joystick::printMotorInput()
{
	Serial.print( "r: " );
	Serial.print( rightPotentiometerReading );
	Serial.print("\n\r");
	Serial.print( "l: " );
	Serial.print( leftPotentiometerReading );
	Serial.print("\n\r");
	delay( 200 );
}

int Joystick::getLeftPower()
{
	leftPotentiometerReading = analogRead( LEFT_JOYSTICK_PIN );

	//setup duty cycle (three speed motor)
	if( leftPotentiometerReading > 719 || leftPotentiometerReading < 495 )
	{
		l_power = FULL_DUTY;
	}
	else if( leftPotentiometerReading > 643|| leftPotentiometerReading < 527 )
	{
		l_power = HALF_DUTY;
	}
	else
	{
		l_power = OFF_DUTY;
	}

	return l_power;
}

int Joystick::getRightPower()
{
	rightPotentiometerReading = analogRead( RIGHT_JOYSTICK_PIN );

	//setup duty cycle (three speed motor)
	if( rightPotentiometerReading > 630 || rightPotentiometerReading < 388 )
	{
		r_power = FULL_DUTY;
	}
	else if( rightPotentiometerReading > 545 || rightPotentiometerReading < 426 )
	{
		r_power = HALF_DUTY;
	}
	else
	{
		r_power = OFF_DUTY;
	}

	return r_power;
}

int Joystick::getRightDirection()
{
	rightPotentiometerReading = analogRead( RIGHT_JOYSTICK_PIN );
	return ( rightPotentiometerReading > MID_POTENTIOMETER_READING_RIGHT ) ? FORWARD : BACKWARD;
}

int Joystick::getLeftDirection()
{
	leftPotentiometerReading = analogRead( LEFT_JOYSTICK_PIN );
	return ( leftPotentiometerReading > MID_POTENTIOMETER_READING_LEFT ) ? FORWARD : BACKWARD;
}

int Joystick::getServoPosition()
{
	servoPotentiometerReading = analogRead( SERVO_PIN );

	if( servoPotentiometerReading < 200 )
	{
		return RIGHT;
	}
	else if( servoPotentiometerReading < 800)
	{
		return MIDDLE;
	}
	else
	{
		return LEFT;
	}
}
int Joystick::getTurn()
{
	return turn_magnitude;
}

